#include <slam_2d_ros/mapping/occupancy_map.h>

namespace slam_2d{

// 构造函数
OccupancyMap::OccupancyMap(){
    // 构建模板
    for (int x = -model_size_; x <= model_size_; x++) {
        // y = [-400, 400]
        for (int y = -model_size_; y <= model_size_; y++) {
            Model2DPoint pt;
            pt.dx_ = x;
            pt.dy_ = y;
            pt.range_ = sqrt(x * x + y * y) * inv_resolution_;      //计算真实距离
            pt.angle_ = atan2(y, x);       //计算角度
            pt.angle_ = pt.angle_ > M_PI ? pt.angle_ - 2 * M_PI : pt.angle_;  // limit in 2pi
            model_.push_back(pt);
        }
    }  

    // 图像初始化 1000*1000 默认未知值127
    occupancy_grid_ = cv::Mat(image_size_, image_size_, CV_8U, 127); 
    center_image_ = Vec2d(image_size_ / 2, image_size_ / 2);     
}


OccupancyMap::~OccupancyMap(){

}


// 插入一帧雷达数据
void OccupancyMap::AddLidarFrame(boost::shared_ptr<Frame> frame){
    auto& scan = frame->scan_;
    Pose2D pose_in_submap = pose_.inverse() * frame->pose_; 

    has_outside_pts_ = false;

    // 先计算末端点所在坐标
    vector<Vec2i> endpoints;    //此处原始为set，现改为vector 先实现功能，后续调整
    for (size_t i = 0; i < scan->ranges.size(); ++i) {
        // 无效点过滤
        if (scan->ranges[i] < scan->range_min || scan->ranges[i] > scan->range_max) {
            continue;
        }
        // 计算雷达位姿
        double real_angle = scan->angle_min + i * scan->angle_increment;

        double x = scan->ranges[i] * std::cos(real_angle);
        double y = scan->ranges[i] * std::sin(real_angle);

        // endpoints 添加雷达点集转换为图像的坐标
        // Vec2d lidar_pose = frame->pose_ * Vec2d(x, y);
        // Vec2i pos_in_image = World2Image(lidar_pose);
        // cout << "[AddLidarFrame] i = " << i 
        //     << ", x = " << x << ", y = " << y
        //     << ", lidar_pose: " << lidar_pose.transpose()
        //     << ", pos_in_image: " << pos_in_image.transpose()
        //     << endl;

        endpoints.emplace_back(World2Image(frame->pose_ * Vec2d(x, y)));
    }

    // cout << "endpoints size: " << endpoints.size() << endl;
    // 计算起点在图像中坐标（当前位姿）
    Vec2i start = World2Image(frame->pose_.translation());
    // cout << "[AddLidarFrame] start = " << start << endl;
    // 遍历雷达点集，进行直线填充
    for(auto &pt : endpoints){
        // cout << "--> pt: " << pt.transpose() << endl;
        BresenhamFilling(start, pt);
        // SetPoint(pt, true);     // 末端点涂黑
    }   

    // // 遍历雷达点集，进行直线填充
    // std::for_each(std::execution::par_unseq, endpoints.begin(), endpoints.end(),
    //     [this, &start](const auto& pt) { BresenhamFilling(start, pt); });
    // 末端点涂黑
    std::for_each(endpoints.begin(), endpoints.end(), [this](const auto& pt) { SetPoint(pt, true); });   
}

// 设置pose 
void OccupancyMap::SetPose(const Pose2D& pose){
    pose_ = pose;
}

// 设置点值 见P219
void OccupancyMap::SetPoint(const Vec2i& pt, bool occupy) {
    int x = pt[0], y = pt[1];
    if (x < 0 || y < 0 || x >= occupancy_grid_.cols || y >= occupancy_grid_.rows) {
        if (occupy) {
            has_outside_pts_ = true;
        }
        return;
    }

    /// 这里设置了一个上下限
    uchar value = occupancy_grid_.at<uchar>(y, x);
    if (occupy) {
        if (value > 117) {  //观测到障碍物次数+1
            occupancy_grid_.ptr<uchar>(y)[x] -= 1;
        }
    } else {
        if (value < 137) {  //观测到空白次数+1
            occupancy_grid_.ptr<uchar>(y)[x] += 1;
        }
    }
}


// 直线填充
void OccupancyMap::BresenhamFilling(const Vec2i& p1, const Vec2i& p2){
    int dx = p2.x() - p1.x();
    int dy = p2.y() - p1.y();
    int ux = dx > 0 ? 1 : -1;
    int uy = dy > 0 ? 1 : -1;

    dx = abs(dx);
    dy = abs(dy);
    int x = p1.x();
    int y = p1.y();

    if (dx > dy) {
        // 以x为增量
        int e = -dx;
        for (int i = 0; i < dx; ++i) {
            x += ux;
            e += 2 * dy;
            if (e >= 0) {
                y += uy;
                e -= 2 * dx;
            }

            if (Vec2i(x, y) != p2) {
                SetPoint(Vec2i(x, y), false);
            }
        }
    } else {
        int e = -dy;
        for (int i = 0; i < dy; ++i) {
            y += uy;
            e += 2 * dx;
            if (e >= 0) {
                x += ux;
                e -= 2 * dy;
            }
            if (Vec2i(x, y) != p2) {
                SetPoint(Vec2i(x, y), false);
            }
        }
    }
}

// 获取地图数据
cv::Mat OccupancyMap::GetOccupancyGridBlackWhite() const{
    cv::Mat image(image_size_, image_size_, CV_8UC3);
    for (int x = 0; x < occupancy_grid_.cols; ++x) {
        for (int y = 0; y < occupancy_grid_.rows; ++y) {
            if (occupancy_grid_.at<uchar>(y, x) == 127) {           //未知127
                image.at<cv::Vec3b>(y, x) = cv::Vec3b(127, 127, 127);
            } else if (occupancy_grid_.at<uchar>(y, x) < 127) {     //障碍物 0
                image.at<cv::Vec3b>(y, x) = cv::Vec3b(0, 0, 0);
            } else if (occupancy_grid_.at<uchar>(y, x) > 127) {     //空白 255
                image.at<cv::Vec3b>(y, x) = cv::Vec3b(255, 255, 255);
            }
        }
    }
    // cv::imshow("image", image);
    // cv::waitKey(10);
    return image;
}

}; // namespace slam_2d